#ifndef CONFIG_H
#define CONFIG_H

#include <string.h>
using namespace std;
#include <unistd.h>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <nav_msgs/Odometry.h>

#include "get_states/flight_status.h"
#include "get_states/show_status.h"
#include "get_states/flight_status_control.h"


// 飞行状态枚举
enum FlightPhase {
    WAITING,        // 等待     0
    INIT,           // 初始化   1
    ARMING,         // 解锁中   2
    TAKEOFF,        // 起飞中   3
    HOVER,          // 悬停中   4  
    LAND,           // 降落后   5
    DISARM          // 上锁     6
};

// 人为控制飞行状态
FlightPhase int_to_enum(const int statu){
    if(statu==1) return INIT;
    if(statu==5){
        ROS_INFO("Hover complete. Initiating landing...");
        return LAND;
    }
    return WAITING;
}

// 飞行状态播报
string enum_to_str(const FlightPhase phase){
    if(phase == WAITING) return "WAITING";
    if(phase == INIT) return "INITING";
    if(phase == ARMING) return "ARMING";
    if(phase == TAKEOFF) return "TAKEOFF";
    if(phase == HOVER) return "HOVERING";
    if(phase == LAND) return "LANDING";
    if(phase == DISARM) return "DISARM";
    return "UNKNOW";
}

// 飞行状态初始化
FlightPhase phase = WAITING;

// 参数设置
double hover_alt = 2.0;          // 悬停高度(m)
double hover_duration = 10.0;    // 悬停时间(s)
double arm_timeout = 10.0;       // 解锁超时(s)
double takeoff_timeout = 30.0;   // 起飞超时(s)
double half_alt = (hover_alt + 0.4) / 2;  //降落中途点

// 全局变量存储无人机状态
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
    current_state = *msg;
}
// 全局变量存储无人机位置
// geometry_msgs::PoseStamped current_local_pose;
nav_msgs::Odometry current_local_statu;
geometry_msgs::PoseWithCovariance current_local_pose;
void get_local_pose(const nav_msgs::Odometry::ConstPtr& msg) {
    current_local_statu = *msg;
    current_local_pose = current_local_statu.pose;
}

// get_states::flight_status flight_statu;
// void flight_cb(const get_states::flight_status::ConstPtr& msg) {
//     flight_statu = *msg;
//     phase = int_to_enum(flight_statu.statu);
// }
bool flight_cb(get_states::flight_status_control::Request &request, 
        get_states::flight_status_control::Response &response) {
    int statu = request.statu;
    phase = int_to_enum(statu);
    response.success = true;
    return true;
}


// 位掩码组合定义
namespace ControlMasks {
    // 基本忽略位
    const uint16_t IGNORE_PX   = mavros_msgs::PositionTarget::IGNORE_PX;
    const uint16_t IGNORE_PY   = mavros_msgs::PositionTarget::IGNORE_PY;
    const uint16_t IGNORE_PZ   = mavros_msgs::PositionTarget::IGNORE_PZ;
    const uint16_t IGNORE_VX   = mavros_msgs::PositionTarget::IGNORE_VX;
    const uint16_t IGNORE_VY   = mavros_msgs::PositionTarget::IGNORE_VY;
    const uint16_t IGNORE_VZ   = mavros_msgs::PositionTarget::IGNORE_VZ;
    const uint16_t IGNORE_AFX  = mavros_msgs::PositionTarget::IGNORE_AFX;
    const uint16_t IGNORE_AFY  = mavros_msgs::PositionTarget::IGNORE_AFY;
    const uint16_t IGNORE_AFZ  = mavros_msgs::PositionTarget::IGNORE_AFZ;
    const uint16_t IGNORE_YAW  = mavros_msgs::PositionTarget::IGNORE_YAW;
    const uint16_t IGNORE_YAW_RATE = mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

    // 常用组合模式
    const uint16_t VELOCITY_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW | IGNORE_YAW_RATE;

    const uint16_t VERTICAL_VELOCITY_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ | IGNORE_VX | IGNORE_VY |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW | IGNORE_YAW_RATE;

    const uint16_t POSITION_CONTROL = 
        IGNORE_VX | IGNORE_VY | IGNORE_VZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW_RATE;

    const uint16_t VELOCITY_WITH_YAW_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW_RATE;

    const uint16_t ACCELERATION_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_VX | IGNORE_VY | IGNORE_VZ |
        IGNORE_YAW | IGNORE_YAW_RATE;
}

#endif